package com.testcolision.simulation;

import com.badlogic.gdx.math.Matrix4;
import com.badlogic.gdx.math.Vector3;

public class Camera3D {

	public static final int CAMERA_STATE_INTERPOLATE = 0;
	public static final int CAMERA_STATE_ORBIT = 1;
	public static final int CAMERA_STATE_DEFAULT = 2;
	public int state = CAMERA_STATE_INTERPOLATE;

	public Vector3 pos = new Vector3(0, 0, 0);
	public Vector3 direction = new Vector3(0, 0, 0);
	public Vector3 upVec = new Vector3(0, 0, 0);
	public Matrix4 rotation = new Matrix4();

	public Vector3 playerPos = new Vector3(0, 0, 0);

	public Vector3 targetPos = new Vector3(0, 0, 0);

	public Vector3 cameraVel = new Vector3(0, 0, 0);

	private float alpha = 0.5f;
	private float rotationAngle = 0;
	private Vector3 lerpDirection = new Vector3(0, 0, 0);

	public Camera3D(Vector3 p) {
		pos.set(p);
	}

	float k = -10f;
	float relDist;
	float relVel;
	Vector3 unitAxis = new Vector3();
	public float yaw;
	public float pitch = 6;

	public void update(Vector3 p, Vector3 up, Vector3 d, Vector3 sideVec, float delta) {
		playerPos.set(p);
		direction.set(d).nor();

		rotation.setFromEulerAngles(yaw, 0, 0);
		direction.rot(rotation);

		pos.set(playerPos).sub(direction);
		upVec.set(up);
		pos.add(upVec.tmp().nor().mul(8f));
	}

	public void orbit(Vector3 p, Vector3 up, Vector3 d, float speed) {
		playerPos.set(p);
		direction.set(d).nor().mul(6);

		rotation.setToRotation(up, rotationAngle++);

		if (rotationAngle > 360)
			rotationAngle -= 360;

		direction.rot(rotation);
		upVec.set(up);

		pos.set(playerPos).sub(direction);
		pos.add(upVec.tmp().nor().mul(1.5f));
	}

	public void interpolate(Vector3 p, Vector3 up, Vector3 d, float speed) {
		lerpDirection.set(d).nor().mul(6);
		targetPos.set(p).sub(lerpDirection);
		pos.lerp(targetPos, 0.1f);
		direction.set(targetPos.sub(pos)).nor();
		upVec.set(up);
	}

	public void rotate(float yawInc, float pitchInc) {
		this.yaw += yawInc;
		this.pitch += pitchInc;
		// if (pitch < -90)
		// pitch = -90;
		// if (pitch > 90)
		// pitch = 90;
		//

	}

	// float CriticallyDampedSpring(float a_Target, float a_Current, float
	// a_Velocity, float a_TimeStep) {
	// float currentToTarget = a_Target - a_Current;
	// float springForce = currentToTarget * SPRING_CONSTANT;
	// float dampingForce = (float) (-a_Velocity * 2 *
	// Math.sqrt(SPRING_CONSTANT));
	// float force = springForce + dampingForce;
	// a_Velocity += force * a_TimeStep;
	// float displacement = a_Velocity * a_TimeStep;
	//
	// return displacement;
	// }

	// public void interpolate(Vector3 p, Vector3 up, Vector3 d, Vector3 vel,
	// float delta) {
	// unitAxis.set(p).sub(pos);
	// direction.set(unitAxis);
	// relDist = unitAxis.len() - 3;
	// unitAxis.nor();
	// relVel = cameraVel.cpy().sub(vel).dot(unitAxis);
	// Vector3 f = unitAxis.mul(relDist).mul(k).add(relVel);
	// cameraVel.add(f);
	// pos.sub(cameraVel.mul(delta));
	// upVec.set(up);
	// }
}
